
#include <ServoPWMF.h>
#include <WString.h>  
//Bender variable
#include "WProgram.h"
void setup();
void loop();
void sampleEMG();
void performAction();
void sampleBender();
unsigned long lastCommandTime=0;
unsigned long lastLRSampleTime=0;
//Time = OriginalTime * 61.03515625 / 976.5625
unsigned int benderSampleRate = 150 * 61.03515625 / 976.5625; //100ms cmd sampling rate;
int RTV;
int LTV;
int RPin = 1;
int LPin = 0;
String LRCmd = String(20);       // allocate a new String
boolean lastLR = false;
int RbenderVal;
int LbenderVal;

// EMG variable 
boolean emgAvailable = false;
int EMGPin = 5;

ServoPWMF s;

void setup() {
  Serial.begin(9600);
  s.setupTimer();
  int RDTV = analogRead(RPin);
  int LDTV = analogRead(LPin);
  RTV = RDTV + 30;
  LTV = LDTV + 30;
  Serial.print("RDTV:");
  Serial.print(RDTV);
  Serial.print("  LDTV:");
  Serial.println(LDTV);
}


void loop() {

  sampleBender();
  sampleEMG();
  performAction();

}

void sampleEMG()
{
  int emgVal = analogRead(EMGPin);
  if( emgVal > 20 )
  {
   emgAvailable = true; 
   
  Serial.println(emgVal);
  }
}

//does not perform action if cmdAvailable == false
void performAction()
{

  if( emgAvailable)
  {
   // Serial.print("Cmd:");
  //  Serial.println(LRCmd);
    if(!strcmp(LRCmd, "R"))
    {
      Serial.println("GRAB");
    }
    if(!strcmp(LRCmd, "RR"))
    {
      Serial.println("PINCH");
    }
     LRCmd = "";
     emgAvailable = false; 
    }
  
}



void sampleBender()
{

  if( (millis() - lastLRSampleTime) > benderSampleRate) //delay 100ms before we take next sample
  {
    RbenderVal = analogRead(RPin);   
    LbenderVal = analogRead(LPin);    
   // Serial.println(RbenderVal);
    if(!lastLR)
    {
      if(RbenderVal > RTV)
      {
        lastLR = true; 
        LRCmd +='R';
        Serial.print("Right: "); 
        Serial.println(RbenderVal);
        Serial.println(LRCmd);
      }
      if(LbenderVal > LTV)
      {
        lastLR = true;
        LRCmd +='L';
        Serial.print("Left: "); 
        Serial.println(LbenderVal);
        Serial.println(LRCmd);
      }
    }

    if(  lastLR&& RbenderVal< RTV-15 && LbenderVal < LTV - 15)
    {
      lastLR = false;
      //Serial.println("middle");
    }

    lastLRSampleTime = millis();
  }
}

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

